﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Core.Mathematics;
using GPSCrowdSource.DataStructures;

namespace GPSCrowdSource.Algorithms {
  
  /// <summary>
  /// Provides a set of calculations that are common to geodesics.
  /// </summary>
  public abstract class GeoUtils {
    

    /// <summary>
    /// Calculate the archaversine.
    /// </summary>
    public static double ArcHS(double v) {
      return 2.0*Math.Asin(Math.Sqrt(v));
      }

    /// <summary>
    /// Haversine
    /// </summary>
    public static double HS(double theta) {
      return Math.Sin(theta/2.0d)*Math.Sin(theta/2.0d);
      }

    /// <summary>
    /// Calculate the spherical distance between two polar (latitude/longitude) values in
    /// nautical miles.  The given value are assumed to be in decimal degrees.
    /// </summary>
    public static double GetDistance(GeoPosition inA, GeoPosition inB) {
      return GetDistance(inA.Latitude,inA.Longitude,inB.Latitude,inB.Longitude);
      }

    /// <summary>
    /// Calculate the spherical distance between two polar (latitude/longitude) values in
    /// nautical miles.  The given value are assumed to be in decimal degrees.
    /// </summary>
    public static double GetDistance(double lat1, double lon1, double lat2, double lon2) {
      // hs(d/r) = hs(lon2-lon1) + cos(lon2)cos(lon1)hs(lat2-lat1);
      lon1 = lon1*GeoConstants.D2R;
      lon2 = lon2*GeoConstants.D2R;
      lat1 = lat1*GeoConstants.D2R;
      lat2 = lat2*GeoConstants.D2R;

      double v1 = HS(lat2-lat1) + Math.Cos(lat1)*Math.Cos(lat2)*HS(lon2-lon1);
      return (float)(ArcHS(v1)*GeoConstants.EARTH_RADIUS_NM);
      }

    /// <summary>
    /// Conver the polar values to an ECEF reference coordinate.
    /// </summary>
    /// <param name="alt">The altitude in feet</param>
    /// <param name="lat">The latitude in decimal degrees</param>
    /// <param name="lon">The longitude in decimal degrees</param>
    /// <returns>ECEF cartesian coordinates</returns>
    public static Vector3 ToVectorECEF(double lat, double lon, double alt) {    // DEBUG param
      double x,y,z;
      double r = alt + GeoConstants.EARTH_RAD_WGS84_FT;
      lat = lat*GeoConstants.D2R;
      lon = lon*GeoConstants.D2R;
    
      x = r*Math.Cos(lat)*Math.Cos(lon);
      y = r*Math.Sin(lon)*Math.Cos(lat);
      z = r*Math.Sin(lat);

      return new Vector3((float)x,(float)y,(float)z);
      }
    
    /// <summary>
    /// Create a geographic (polar latitude/longitude) position from an (x,y,z) ECEF vector.
    /// 
    /// (This assumes the position is in feet from the earths center)
    /// 
    /// ECEF is where the Z axis goes straight up, the X axis faces toward the user
    /// and y is to the right.
    /// 
    ///      (z)
    ///       |
    ///       |
    ///       |
    /// ---------------- (y)
    ///      /| 
    ///     / | 
    ///   (x) |
    /// 
    /// </summary>
    /// <returns>A vector where x,y,z are longitude, latitude and altitude respectvely.
    ///  </returns>
    public static Vector3 ToVectorLatLon(double x, double y, double z) {
      float lat = 0;
      float lon = 0;
      float alt = 0;
      
      double r = Math.Sqrt(x*x + y*y + z*z);

      if(x != 0 && z != 0) {
        lat = (float)(Math.Asin(z/r)*GeoConstants.R2D);
        }
    
      if(x != 0 || y != 0) lon = (float)(Math.Atan2(y,x)*GeoConstants.R2D);


      alt = (float)(Math.Sqrt( Math.Pow(x,2) + Math.Pow(y,2) + Math.Pow(z,2))-GeoConstants.EARTH_RAD_WGS84_FT);

      return new Vector3(lon,lat,alt);
      }
      
    /// <summary>
    /// Get the center position.
    /// </summary>
    public static GeoPosition GetAverage(GeoPosition[] inPos) {
      float lat = 0;
      float lon = 0;
      float alt = 0;
      double ticks = 0;
      
      for(int i=0;i<inPos.Length;i++) {
        lat += inPos[i].Latitude;
        lon += inPos[i].Longitude;
        alt += inPos[i].Altitude;
        ticks += inPos[i].Time.Ticks;
        }

      return new GeoPosition(new DateTime((long)(ticks/((double)inPos.Length))),
                              lat/((float)inPos.Length),
                              lon/((float)inPos.Length),
                              alt/((float)inPos.Length));
      }

    /// <summary>
    /// Create a distribution that takes a set of GPS data points and returns
    /// a distrubtion where sample element is the largest change in group size from
    /// one hour to another.
    /// </summary>
    /// <param name="inSource">The data source (GPS data)</param>
    /// <param name="maxHoursForward">Only compare any given hour with less than or equal to this many hours ahead in time</param>
    public static HourToVolumeCollection CreateHourToVolumeDistribution(IDataSource inSource, int maxHoursForward) {
      
      TimeSpaceBin inBins = new TimeSpaceBin(inSource);

      // Determine the expected radial mean.
      List<RadialSample> rmph = new List<RadialSample>();
      
      // Get the collection for each bin and compute the best radius per bin.
      foreach(TimeSpaceCollection ts in inBins) {
        
        if(ts.Count > 0) {
          Vector3 radii = ts.Radii;
          rmph.Add(new RadialSample(radii.Z,ts.Bin,ts.Bin));
          }

        }
      
      // Create the radial sample distribution where each
      // delta is the large delta in radius
      for (int i=0;i<rmph.Count;i++) {
        int lI = i;
        float lR = float.MinValue;

        for(int j=i+1;j<rmph.Count;j++) {
          
          if((rmph[j].A.Duration - rmph[i].A.Duration).TotalHours > maxHoursForward) continue;

          float t = (rmph[j].Delta-rmph[i].Delta)/rmph[i].Delta;

          if(lR == float.MinValue || Math.Abs(t) > Math.Abs(lR)) {
            lI = j;
            lR = t;
            }
          }

        RadialSample rs = rmph[i];
        rs.Delta = lR;
        rs.B = rmph[lI].A;
        rmph[i] = rs;
        }
      
      // Remove elements
      for(int i=0;i<rmph.Count;i++) {
        if(rmph[i].Delta == float.MinValue) {
          rmph.RemoveAt(i);
          i--;
          } 
        }

      return new HourToVolumeCollection(rmph);
      }

    }

  }
